// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.


#pragma once

#include <libvis/libvis.h>
#include <libvis/rgbd_video.h>
#include <libvis/sophus.h>

#include "badslam/direct_ba.h"
#include "badslam/keyframe.h"

namespace vis {

// Given a DirectBA instance, stores the current keyframe poses in the provided
// vector.
void RememberKeyframePoses(
    DirectBA* dense_ba,
    vector<SE3f>* original_keyframe_T_global);

// Given previously stored keyframe poses (with RememberKeyframePoses()) and the
// current keyframe poses (from the given DirectBA instance), deforms the poses
// of non-keyframes in the rgbd_video (within the range
// [start_frame, end_frame]) to match the changes in keyframe poses from the old
// to the current state.
void ExtrapolateAndInterpolateKeyframePoseChanges(
    u32 start_frame,
    u32 end_frame,
    DirectBA* dense_ba,
    const vector<SE3f>& original_keyframe_T_global,
    RGBDVideo<Vec3u8, u16>* rgbd_video);

}
